#include "fake_laser/FakeLaser.h"
#include <pcl_conversions/pcl_conversions.h>
#include "occupancyMap/OccupancyGridMapRosConverter.h"
#include "ros_utils/transform.h"
#include "ros_utils/pointcloud.h"

FakeLaser::FakeLaser()
: Node("fake_laser")
{
  try {
    // 初始化参数之前先声明
    this->declare_parameter("map_topic", "/map");
    this->declare_parameter("laser_topic", "/laser_fake");
    this->declare_parameter("pose_topic", "/pose");
    this->declare_parameter("base_frameId", "base_link");
    this->declare_parameter("odom_frameId", "odom");
    this->declare_parameter("map_frameId", "map");
    this->declare_parameter("laser_frameId", "laser");
    this->declare_parameter("laser_range_min", 0.05);
    this->declare_parameter("laser_range_max", 10.0);
    this->declare_parameter("laser_offset_x", 0.0);
    this->declare_parameter("laser_offset_y", 0.0);
    this->declare_parameter("laser_offset_z", 0.5);

    // 获取参数
    map_topic_ = this->get_parameter("map_topic").as_string();
    laser_topic_ = this->get_parameter("laser_topic").as_string();
    pose_topic_ = this->get_parameter("pose_topic").as_string();
    baselink_frame_ = this->get_parameter("base_frameId").as_string();
    odom_frame_ = this->get_parameter("odom_frameId").as_string();
    map_frame_ = this->get_parameter("map_frameId").as_string();
    laser_frame_ = this->get_parameter("laser_frameId").as_string();
    laser_range_min_ = this->get_parameter("laser_range_min").as_double();
    laser_range_max_ = this->get_parameter("laser_range_max").as_double();
    laser_offset_x_ = this->get_parameter("laser_offset_x").as_double();
    laser_offset_y_ = this->get_parameter("laser_offset_y").as_double();
    laser_offset_z_ = this->get_parameter("laser_offset_z").as_double();
  
    // 初始化激光生成器
    laser_generator_.setMinRange(laser_range_min_);
    laser_generator_.setMaxRange(laser_range_max_);
  }
  catch (const std::exception& e) {
    RCLCPP_ERROR(this->get_logger(), "Error during initialization: %s", e.what());
    throw;
  }
}

bool FakeLaser::init()
{
  try {
    // 初始化 transform handler
    tf_ = std::make_shared<utils::TransformHandler>(shared_from_this());

    // 创建订阅者
    sub_map_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
      map_topic_, 
      rclcpp::QoS(1),
      std::bind(&FakeLaser::mapCallback, this, std::placeholders::_1));

    // 创建发布者
    pub_laser_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
      laser_topic_, 
      rclcpp::QoS(1));
    
    pub_pose_ = this->create_publisher<agv_msgs::msg::PoseWithConfidence>(
      pose_topic_, 
      rclcpp::QoS(1));

    // 创建定时器
    pose_update_timer_ = this->create_wall_timer(
      std::chrono::milliseconds(10),
      std::bind(&FakeLaser::updateLaserPose, this));

    pub_laser_timer_ = this->create_wall_timer(
      std::chrono::milliseconds(50),
      std::bind(&FakeLaser::generateFakeLaser, this));

    RCLCPP_INFO(this->get_logger(), "FakeLaser node initialized successfully");
    return true;
  }
  catch (const std::exception& e) {
    RCLCPP_ERROR(this->get_logger(), "Error during initialization: %s", e.what());
    return false;
  }
}

void FakeLaser::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
{
  if (OccupancyGridMapRosConverter::fromOccupancyGridMsg(*msg, occupancy_map_)) {
    sub_map_ = nullptr;  // equivalent to ROS1's shutdown()
    pub_laser_timer_->reset();  // equivalent to ROS1's start()
  }
}

void FakeLaser::updateLaserPose()
{
  geometry_msgs::msg::Transform laser2Base;
  laser2Base.translation.x = laser_offset_x_;
  laser2Base.translation.y = laser_offset_y_;
  laser2Base.translation.z = laser_offset_z_;
  laser2Base.rotation = utils::tf::getQuaternionMsgFrom(0, 0, 0);

  tf_->sendTransform(laser2Base, baselink_frame_, laser_frame_, this->now());

  auto [has_transform_l2m, laser2Map] = tf_->getTransform(laser_frame_, map_frame_);
  if (!has_transform_l2m)
    return;

  const auto& laser_pose_x = laser2Map.transform.translation.x;
  const auto& laser_pose_y = laser2Map.transform.translation.y;
  const auto& laser_height = laser2Map.transform.translation.z;

  Eigen::Vector2d laserPosition_in_map(laser_pose_x, laser_pose_y);
  laser_generator_.setLaserOrigin(laserPosition_in_map, laser_height);

  agv_msgs::msg::PoseWithConfidence msg;
  msg.header = laser2Map.header;
  msg.score = 100;
  msg.pose.pose.position.x = laser2Map.transform.translation.x;
  msg.pose.pose.position.y = laser2Map.transform.translation.y;
  msg.pose.pose.position.z = laser2Map.transform.translation.z;
  msg.pose.pose.orientation = laser2Map.transform.rotation;

  pub_pose_->publish(msg);
}

void FakeLaser::generateFakeLaser()
{
  auto ray_laser = std::make_shared<pcl::PointCloud<pcl::PointWithRange>>();
  ray_laser->header.frame_id = map_frame_;
  if (!laser_generator_.doRayCasting(occupancy_map_, *ray_laser))
  {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(),
      *(this->get_clock()),
      1000,  // milliseconds
      "Failed to generate fake laser scan. Laser is out of map.");
    return;
  }

  auto [has_transform_m2l, map2laser] = tf_->getTransform(map_frame_, laser_frame_);
  if (!has_transform_m2l)
    return;

  auto fake_laser_raw = utils::pcl::transformPointcloud<pcl::PointWithRange>(ray_laser, map2laser);

  // filter pointcloud
  auto fake_laser_range_filtered = utils::pcl::filterPointcloudByRange2D<pcl::PointWithRange>(
      fake_laser_raw, laser_generator_.getMinRange(), laser_generator_.getMaxRange());

  // Typical 2D LiDAR specification: -135 ~ 135 degree
  auto fake_laser_angle_filtered =
      utils::pcl::filterPointcloudByAngle<pcl::PointWithRange>(fake_laser_range_filtered, -135, 135);

  sensor_msgs::msg::PointCloud2 msg;
  pcl::toROSMsg(*fake_laser_angle_filtered, msg);
  pub_laser_->publish(msg);
}